package allrobots.TraceAndAvoid;

import allrobots.Robot.RobotMessage;
import lombok.Getter;
import t10086.Data.Map.GridType;
import t10086.Data.Map.T10086Map;
import t10086.Data.Map.TextCoordinate;

import java.util.*;
import java.util.List;

// 1.根据传入的地图找总路线
// 2.找下一步要走的坐标

public class Route {
    private final Octree start;
    private final Octree end;
    private static int rows, cols; // 行列
    private final T10086Map map; // 地图
    //打开列表和关闭列表
    private PriorityQueue<Octree> openList = new PriorityQueue<>();
    private List<Octree> closeList = new ArrayList<>();
    @Getter
    private List<TextCoordinate> finalRoute = new ArrayList<>(); // 最终路径
    @Getter
    private TextCoordinate nextStep = new TextCoordinate(); // 下一步往哪走

    // 计算曼哈顿距离
    public int calMD(TextCoordinate curPos) {
        return Math.abs(curPos.x - end.pos.x) + Math.abs(curPos.y - end.pos.y);
    }

    public Octree findNodeInOpen(TextCoordinate textCoordinate) {
        if (textCoordinate == null || openList.isEmpty()) return null;
        for (Octree temp : openList) {
            if (temp.pos.equals(textCoordinate)) {
                return temp;
            }
        }
        return null;
    }

    //看这个坐标的结点在不在关闭列表中
    public boolean isInClose(TextCoordinate current) {
        if (closeList.isEmpty()) return false;
        for (Octree temp : closeList) {
            if (temp.pos.equals(current)) {
                return true;
            }
        }
        return false;
    }

    //能不能加入打开列表
    public boolean canAddInOpen(TextCoordinate pos) {
        if (pos.x < 0 || pos.x >= rows || pos.y < 0 || pos.y >= cols) {
            return false;
        }
        if (!map.getGrid(pos).getGridType().equals(GridType.NULL)) {
            return false;
        }
        return !isInClose(pos);
    }

    public void addNeighbors(Octree current) {
        int x = current.pos.x;
        int y = current.pos.y;
        add(current, x - 1, y);
        add(current, x + 1, y);
        add(current, x, y - 1);
        add(current, x, y + 1);
    }

    public void add(Octree current, int xNext, int yNext) {//current是现在的;x,y是孩子
        if (canAddInOpen(new TextCoordinate(xNext, yNext))) {
            TextCoordinate childPoint = new TextCoordinate(xNext, yNext);
            Octree child = findNodeInOpen(childPoint);
            int forwardCost = current.forwardCost + 1;
            int towardCost = calMD(childPoint);
            //每次更新要找打开列表里的child更新，没有就加上
            if (child == null) {//打开列表里没找到结点
                if (end.pos.equals(childPoint)) {//这个点是终点
                    child = end;
                    child.parent = current;
                } else {//这个点不是终点
                    child = new Octree(childPoint);
                    child.parent = current;
                }
                child.forwardCost = forwardCost;
                child.towardCost = towardCost;
                openList.add(child);
            } else if (child.forwardCost > forwardCost) {//打开列表里有这个点且前向代价更大：更新表里的信息
                child.forwardCost = forwardCost;
                child.parent = current;
                openList.add(child);
            }
        }
    }

    // 判断可不可以走
    public void traceRoute() {
        //System.out.println("------------------------------------这是一次寻路----------------------------------------------------");
        //开始寻路
        openList.add(start);
        while (!openList.isEmpty()) {
            //System.out.println(count+"||||||||||||||||||||||||||||||||||这是一次循环||||||||||||||||||||||||||||||||||||||||||");
            Octree current = openList.poll();
            closeList.add(current);
            addNeighbors(current);
        }
        //输出路径
        if (isInClose(end.pos)) {
            Stack<Octree> pathStack = new Stack<>();
            Octree pCur = end;
            while (pCur != null) {
                pathStack.push(pCur);
                pCur = pCur.parent;
            }

            finalRoute = new ArrayList<>();
            while (!pathStack.isEmpty()) {
                TextCoordinate p = pathStack.pop().pos;
                finalRoute.add(p);
            }
            nextStep = finalRoute.size() > 1 ? finalRoute.get(1) : finalRoute.get(0);
        } else {
            finalRoute.add(start.pos);
            nextStep = start.pos;
        }
    }


    public Route(RobotMessage robot, T10086Map map) {
        rows = map.getRows();
        cols = map.getColumns();
        start = new Octree(robot.getPosition());
        end = new Octree(robot.getTermination());
        this.map = map;
        traceRoute();
    }
}
